//
// Created by ZhaoXiaoFei on 2022/8/18.
//

#ifndef ESKF_LOCALIZATION_BACK_END_HPP
#define ESKF_LOCALIZATION_BACK_END_HPP
#include "preDefineData.hpp"
#include "model/imuPredict.hpp"
#include "model/gpsCorrect.hpp"
#include <memory>
#include <model/init.hpp>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
class LocalizationBackEnd{
public:
    LocalizationBackEnd(const double acc_noise, const double gyr_noise,
                        const double acc_bias_noise, const double gyr_bias_noise);

    void processImuData(ImuData imu_data);
    void processGpsData(GpsPositionData gps_data);
    void processMagData(MagData mag_data);

    State* getState();
    geometry_msgs::Pose getFusedPose();
    sensor_msgs::NavSatFix getFusedFix();
    nav_msgs::Odometry getFusedOdometry();

private:
    std::unique_ptr<Initializer> initializer_;
    std::unique_ptr<ImuPredict> imuPredict_;
    std::unique_ptr<GpsCorrect> gpsCorrect_;
    State state;
};

#endif //ESKF_LOCALIZATION_BACK_END_HPP
